

#include "slep.h"
#include "hr_time.h"
#include "epph.h"
#include "Logistic.h"

FUNVAL LogisticC(double *A, double* y, double z, OPTS opt);
FUNVAL LogisticCm1(double *A, double* y, double z, OPTS opt);
FUNVAL LogisticCm2(double *A, double* y, double z, OPTS opt);

FUNVAL LogisticC(double *A, double* y, double z, OPTS opt){
	if( opt.lFlag == 0)
		return LogisticCm1(A,y,z,opt);
	if( opt.lFlag == 1)
		return LogisticCm2(A,y,z,opt);
}


FUNVAL LogisticCm1(double *A, double* y, double z, OPTS opt){
	FUNVAL funret;
	int m,n,i,iterStep, bFlag, steps;
	double rsL2,lambda0,L,cp,c,ccp, alphap,alpha, sc , beta, fun_x, fun_s, m1,m2, fVal, fValp,r_sum, l_sum, gc, root;

	double *x, *xp, *xxp, *Ax, *aa, *bb, *weight, *weighty, *Axp, *ValueL, *funVal, *s, *v, *g, *b, *As, *prob ;


	m = opt.m;
	n = opt.n;


	b = (double * ) malloc( sizeof(double) * n);
	weight = (double * ) malloc( sizeof(double) * m);
	x = (double * ) malloc( sizeof(double) * n);
	Ax = (double * ) malloc( sizeof(double) * m);
	weighty = (double * ) malloc( sizeof(double) * m);
	xp = (double * ) malloc( sizeof(double) * n);
	Axp = (double * ) malloc( sizeof(double) * m);
	xxp = (double * ) malloc( sizeof(double) * n);
	s = (double * ) malloc( sizeof(double) * n);
	As = (double * ) malloc( sizeof(double) * m);
	aa = (double * ) malloc( sizeof(double) * m);
	bb = (double * ) malloc( sizeof(double) * m);
	prob = (double * ) malloc( sizeof(double) * m);
	g = (double * ) malloc( sizeof(double) * n);
	v = (double * ) malloc( sizeof(double) * n);
	ValueL = (double * ) malloc( sizeof(double) * opt.maxIter);
	funVal = (double * ) malloc( sizeof(double) * opt.maxIter);

	initNormalization(&opt,A);

	m1 = normalizeWeights(&opt,weight,y,b);
	m2 = 1.0 - m1;

	rsL2 = opt.rsL2;

	if( opt.init == 2){
		memset(x,0,sizeof(double)*n);
		c = 0.0;
	}
	else{
		if( hasGot(&opt,"x0"))
			dcopy(n,opt.x0,1,x,1);
		else
			memset(x,0,sizeof(double)*n);
		if( hasGot(&opt,"c0"))
			c = opt.c0;
		else
			c = 0.0;
	}
	normalizedmv('N',&opt,A,x,Ax);

	// Nemirovski's line search
	lambda0 = 0.0;

	bFlag = 0;

	L = 1.0/m + rsL2;

	// weighty = weight .* y;
	dsbmv('U',m,0,1.0,weight,1,y,1,0.0,weighty,1);

	// xp = x; Axp = Ax; xxp = zeros; cp = c; ccp = 0;
	dcopy(n,x,1,xp,1);
	dcopy(m,Ax,1,Axp,1);
	memset(xxp,0,sizeof(double)*n);
	cp = c;
	ccp = 0.0;

	alphap = 0.0;
	alpha = 1.0;

	for( iterStep = 0; iterStep< opt.maxIter ; iterStep++){
		beta = (alphap - 1.0)/alpha;
		// s = x + beta * xxp; sc = c + beta * ccp;
		dcopy(n,x,1,s,1);
		daxpy(n,beta,xxp,1,s,1);
		sc = c + beta * ccp;

		// As = Ax + beta *( Ax - Axp);
		dcopy(m,Ax,1,As,1);
		dscal(m,1.0 + beta,As,1);
		daxpy(m,-beta,Axp,1,As,1);

		// aa = -y.* (As+ sc);
		dcopy(m,As,1,aa,1);
		daxpy(m,sc,oneVector,1,aa,1);
		dtbmv('U','N','N',m,0,y,1,aa,1);
		dscal(m,-1.0,aa,1);

		// fun_s= weight' * ( log( exp(-bb) +  exp(aa-bb) ) + bb )+...
        //    rsL2/2 * s'*s;
		fun_s = 0;
		for(i=0; i<m;i++){
			bb[i] = maxof2(aa[i],0.0);
			fun_s += weight[i] * ( log(exp(-bb[i])+exp(aa[i]-bb[i])) + bb[i]);
		}
		fun_s += rsL2 * ddot(n,s,1,s,1)/2.0;

		//b = -weighty .* (1-prob);
		for(i=0;i<m;i++){
			prob[i] = 1.0/(1.0+exp(aa[i]));
			b[i] = - weighty[i] * (1.0 - prob[i]);
		}


		// gc =  sum(b); the gradient of c
		gc = ddot(m,b,1,oneVector,1);

		normalizedmv('T',&opt,A,b,g);

		// g = g + rsL2 * s;
		daxpy(n,rsL2,s,1,g,1);

		// xp = x; Axp = Ax; 
		//cp = c;
		dcopy(n,x,1,xp,1);
		dcopy(m,Ax,1,Axp,1);
		cp = c;
		for(;;){
			// v = s - g/L;
			// c = sc - gc/L;
			dcopy(n,s,1,v,1);
			daxpy(n,-1.0/L,g,1,v,1);

			c = sc - gc/L;

			eplb(x,&root,&steps,v,n,z,lambda0);
			lambda0 = root;

			// v = x - s;
			dcopy(n,x,1,v,1);
			daxpy(n,-1.0,s,1,v,1);

			normalizedmv('N',&opt,A,x,Ax);

			// aa = -y .* (Ax+c);
			dcopy(m,Ax,1,aa,1);
			daxpy(m,c,oneVector,1,aa,1);
			dtbmv('U','N','N',m,0,y,1,aa,1);
			dscal(m,-1.0,aa,1);

			// bb= max(aa,0);
			// fun_s= weight' * ( log( exp(-bb) +  exp(aa-bb) ) + bb )+...
			//    rsL2/2 * s'*s;
			fun_x = 0;
			for(i=0; i<m;i++){
				bb[i] = maxof2(aa[i],0.0);
				fun_x += weight[i] * ( log(exp(-bb[i])+exp(aa[i]-bb[i])) + bb[i]);
			}
			fun_x += rsL2 * ddot(n,x,1,x,1)/2.0;

			r_sum = (ddot(n,v,1,v,1) + (c-sc)* (c - sc))/ 2.0;
			l_sum = fun_x - fun_s - ddot(n,v,1,g,1) - (c-sc)*gc;

			if( r_sum <= 1.0e-20){
				bFlag = 1;
				break;
			}

			if( l_sum <= r_sum * L)
				break;
			else
				L = maxof2( 2*L, l_sum/r_sum);
		}

		alphap = alpha;
		alpha = (1.0 + sqrt(4.0 * alpha * alpha + 1))/2.0;

		ValueL[iterStep]= L;

		// xxp = x - xp; ccp = c - cp;
		dcopy(n,x,1,xxp,1);
		daxpy(n,-1.0,xp,1,xxp,1);
		ccp = c - cp;

		fValp = fVal = funVal[iterStep] = fun_x;

		if(iterStep != 0)
			fValp = funVal[iterStep];

		if(bFlag)
			break;

		if(terminationCondition(&opt,fVal,fValp,x,dnrm2(n,xxp,1),dnrm2(n,xp,1),iterStep))
			break;
	}
	funret.errorCode = 0;
	funret.c = c;
	funret.funVal = funVal;
	funret.totIter = iterStep;
	strcpy(funret.type,"LogisticR");
	funret.x = x;
	funret.ValueL = ValueL;
	return funret;
}









FUNVAL LogisticCm2(double *A, double* y, double z, OPTS opt){

	int m,n,iterStep,i, bFlag, steps;
	double m1, rsL2,m2, sc, l_sum, r_sum, fun_x, fun_s, L , gc, cnew, c, cp, gamma, ccp , tao, alphap , alpha, fVal, fValp, lambda0, root, beta;
	FUNVAL funret;
	double *weight,*weighty, *aa,*bb, *x, *xp, *xxp, *xnew, *Ax, *Axp, *Axnew, *funVal, *ValueL, *s, *v, *g, *prob, *As, *b;


	m = opt.m;
	n = opt.n;

	b = (double * ) malloc( sizeof(double) * n);
	weight = (double * ) malloc( sizeof(double) * m);
	x = (double * ) malloc( sizeof(double) * n);
	Ax = (double * ) malloc( sizeof(double) * m);
	weighty = (double * ) malloc( sizeof(double) * m);
	xp = (double * ) malloc( sizeof(double) * n);
	Axp = (double * ) malloc( sizeof(double) * m);
	xxp = (double * ) malloc( sizeof(double) * n);
	s = (double * ) malloc( sizeof(double) * n);
	As = (double * ) malloc( sizeof(double) * m);
	aa = (double * ) malloc( sizeof(double) * m);
	bb = (double * ) malloc( sizeof(double) * m);
	prob = (double * ) malloc( sizeof(double) * m);
	g = (double * ) malloc( sizeof(double) * n);
	v = (double * ) malloc( sizeof(double) * n);
	xnew = (double * ) malloc( sizeof(double) * n);
	Axnew = (double * ) malloc( sizeof(double) * m);
	ValueL = (double * ) malloc( sizeof(double) * opt.maxIter);
	funVal = (double * ) malloc( sizeof(double) * opt.maxIter);

	initNormalization(&opt,A);

	m1 = normalizeWeights(&opt,weight,y,b);
	m2 = 1.0 - m1;

	rsL2 = opt.rsL2;

	if( opt.init == 2){
		memset(x,0,sizeof(double)*n);
		c = 0.0;
	}
	else{
		if( hasGot(&opt,"x0"))
			dcopy(n,opt.x0,1,x,1);
		else
			memset(x,0,sizeof(double)*n);
		if( hasGot(&opt,"c0"))
			c = opt.c0;
		else
			c = 0.0;
	}
	normalizedmv('N',&opt,A,x,Ax);

	// Adaptive line search

	lambda0 = 0.0;

	bFlag = 0;

	L = 1.0/m + rsL2;

	gamma = 1.0;

	// weighty = weight .* y;
	dsbmv('U',m,0,1.0,weight,1,y,1,0.0,weighty,1);

	// xp = x; Axp = Ax; xxp = zeros; cp = c; ccp = 0;
	dcopy(n,x,1,xp,1);
	dcopy(m,Ax,1,Axp,1);
	memset(xxp,0,sizeof(double)*n);
	cp = c;
	ccp = 0.0;


	for( iterStep = 0 ; iterStep <n; iterStep++){
		for(;;){
			if(iterStep!=0){
				alpha = ( - gamma + sqrt( gamma * gamma + 4*L*gamma))/ (2*L);			
				beta = (gamma - gamma * alphap)/(alphap * gamma + alphap * L * alpha);

				// s = x + beta * xxp; sc = c + beta * ccp;
				// As = Ax + beta * (Ax - Axp);
				dcopy(n,x,1,s,1);
				daxpy(n,beta,xxp,1,s,1);

				sc = c + beta * ccp;

				dcopy(m,Ax,1,As,1);
				dscal(m,1.0 + beta, Ax,1);
				daxpy(m, - beta, Axp,1,As,1);
			}
			else{
				alpha = (-1.0 + sqrt(5.0) ) / 2.0 ;
				beta = 0.0;
				// s = x; sc = c;
				// As = Ax;
				dcopy(n,x,1,s,1);
				sc = c;
				dcopy(m,Ax,1,As,1);
			}

			// aa = - diag(y) * (As + sc)
			dcopy(m,As,1,aa,1);
			daxpy(m,sc,oneVector,1,aa,1);
			dtbmv('U','N','N',m,0,y,1,aa,1);
			dscal(m,-1.0,aa,1);

			// fun_s= weight' * ( log( exp(-bb) +  exp(aa-bb) ) + bb )+...
			//    rsL2/2 * s'*s;
			fun_s = 0;
			for(i=0; i<m;i++){
				bb[i] = maxof2(aa[i],0.0);
				fun_s += weight[i] * ( log(exp(-bb[i])+exp(aa[i]-bb[i])) + bb[i]);
			}
			fun_s += rsL2 * ddot(n,s,1,s,1);

			//b = -weighty .* (1-prob);
			for(i=0;i<m;i++){
				prob[i] = 1.0/(1.0+exp(aa[i]));
				b[i] = - weighty[i] * (1.0 - prob[i]);
			}

			// gc =  sum(b);
			gc = ddot(m,b,1,oneVector,1);

			// g= A'*b;
			normalizedmv('T',&opt,A,b,g);

			// g = g + rsL2 * s;
			daxpy(n,rsL2,s,1,g,1);

			// v = s - g/L ;
			// cnew = sc - gc/L;
			dcopy(n,s,1,v,1);
			daxpy(n,-1.0/L,g,1,v,1);
			cnew = sc - gc/L;

			eplb(xnew,&root,&steps,v,n,z,lambda0);
			lambda0 = root;

			normalizedmv('N',&opt,A,xnew,Axnew);

			// aa = -diag(y) * ( A*xnew + cnew)
			dcopy(m,Axnew,1,aa,1);
			daxpy(m,cnew,oneVector,1,aa,1);
			dtbmv('U','N','N',m,0,y,1,aa,1);
			dscal(m,-1.0,aa,1);

			fun_x = 0.0;
			for( i = 0; i < m; i++ ){
				bb[i] = maxof2(aa[i],0.0);
				fun_x += weight[i]* ( log( exp(-bb[i]) + exp(aa[i] - bb[i])) + bb[i]);
			}
			fun_x += ddot(n,xnew,1,xnew,1) * rsL2/2.0;

			r_sum = (ddot(n,v,1,v,1) + (cnew - sc)*(cnew - sc))/2.0;
			l_sum = fun_x - fun_s - ddot(n,v,1,g,1) - (cnew - sc) * gc;

			if(r_sum <= 1.0e-20 ){
				bFlag = 1;
				break;
			}

			if( l_sum <= r_sum * L)
				break;
			else
				L = maxof2( 2*L , l_sum/r_sum );
		}

		gamma = L * alpha * alpha;
		alphap = alpha;

		ValueL[iterStep] = L;

		tao = L * r_sum / l_sum;
		if( tao >= 5.0 )
			L = L*0.8 ;

		// xp = x; x = xnew ; xxp = x - xp;
		// Axp = Ax; Ax = Axnew;
		// cp = c; c = cnew; ccp = c - cp;
		dcopy(n,x,1,xp,1);
		dcopy(n,xnew,1,x,1);
		dcopy(n,x,1,xxp,1);
		daxpy(n,-1.0,xp,1,xxp,1);
		dcopy(m,Ax,1,Axp,1);
		dcopy(m,Axnew,1,Ax,1);
		cp = c;
		c = cnew;
		ccp = c - cp;

		fValp = fVal = funVal[iterStep] = fun_x;
		if(iterStep != 0)
			fValp = funVal[iterStep-1];

		if(bFlag)
			break;
		if(terminationCondition(&opt,fVal,fValp,x,dnrm2(n,xxp,1),dnrm2(n,xp,1),iterStep))
			break;
	}

	funret.errorCode = 0;
	funret.c = c;
	funret.funVal = funVal;
	funret.totIter = iterStep;
	strcpy(funret.type,"LogisticR");
	funret.x = x;
	funret.ValueL = ValueL;
	return funret;
}